#!/usr/bin/python3
# coding=UTF-8

import rospy
from std_msgs.msg import Int32
from sensor_msgs.msg import LaserScan
from sensor_msgs.msg import Image
from detection_msgs.msg import Room
import robomaster
from robomaster import robot
from robomaster import camera
from cv_bridge import CvBridge

# 定义相机函数
def camera(ep_camera):
    i = 0
    while True:
        bridge = CvBridge()
        if i <= 4:
            i += 1
            img = ep_camera.read_cv2_image(strategy="newest")
            if img is not None:
                img_msg = bridge.cv2_to_imgmsg(img)
                img_pub.publish(img_msg)
                rospy.loginfo(f"current image:{i}")
            else:
                rospy.logerr("Error!")
        else:
            break
# 相机测试
def camera_test(ep_camera):
    for i in [0,1,2,3,4,5,6,7]:
        # 捕获照片
        rospy.loginfo(f"{i+1} Start!")
        ep_camera.start_video_stream(display=False)
        camera(ep_camera)
        ep_camera.stop_video_stream()
        rospy.loginfo(f"{i+1} Publish!")
        # 防止图像传输不及时
        sleep_time = i + 5
        rospy.sleep(sleep_time)
        rospy.logwarn(f"{i+1} Done!")
# 主函数
if __name__ == '__main__':
    # 初始化节点
    rospy.init_node('ztcar_move')
    # 初始化机器人
    ep_robot = robot.Robot()
    ep_robot.initialize(conn_type="sta")
    ep_chassis = ep_robot.chassis
    ep_camera = ep_robot.camera
    # 发布话题
    img_pub = rospy.Publisher('car_img',Image,queue_size=10)
    # 相机测试
    camera_test(ep_camera)
    # 结束
    ep_robot.close()
